#include "AlignmentDialog.h"

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <vtkDataObjectToTable.h>
#include <vtkElevationFilter.h>
#include <vtkPolyDataMapper.h>
#include <vtkQtTableView.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkVectorText.h>
#include "vtkSmartPointer.h"

#include <QDir>
#include <QFileDialog>
#include <QListView>

#define VTK_CREATE(type, name) \
  vtkSmartPointer<type> name = vtkSmartPointer<type>::New()

using namespace std;



AlignmentDialog::AlignmentDialog(Alignment* a,QWidget* parent) : QDialog(parent), Ui::AlignmentDialog(),alignment(a)
{
    setupUi(this);


	connect(this->pushButton_loadSourceCloud,SIGNAL(clicked()),this,SLOT(loadSourceCloud()));
	connect(this->pushButton_loadTargetCloud,SIGNAL(clicked()),this,SLOT(loadTargetCloud()));
	connect(this->pushButton_nPointAlignment,SIGNAL(clicked()),this,SLOT(nPointAlign()));

	connect(this,SIGNAL(tgtPointPicked(size_t,float,float,float)),alignment,SLOT(tgtPointPicked(size_t,float,float,float)));
	connect(this,SIGNAL(srcPointPicked(size_t,float,float,float)),alignment,SLOT(srcPointPicked(size_t,float,float,float)));

	connect(this->pushButton_tgtPointRemove,SIGNAL(clicked()),this,SLOT(removeTgtPoint()));
	connect(this->pushButton_srcPointRemove,SIGNAL(clicked()),this,SLOT(removeSrcPoint()));

	connect(this->pushButton_icpAlign,SIGNAL(clicked()),alignment,SLOT(icpAlign()));

	tgt_viewer.reset(new pcl::visualization::PCLVisualizer ("3D Viewer Target"));
	tgt_viewer->setBackgroundColor (0, 0, 0);
	tgt_viewer->addCoordinateSystem (1.0);
	tgt_viewer->initCameraParameters ();
	tgt_viewer->registerPointPickingCallback(&AlignmentDialog::tgt_pp_callback,*this, (void*)&tgt_viewer);

	vtkSmartPointer<vtkRenderWindow> renderWindow_tgt = tgt_viewer->getRenderWindow ();
	this->qvtkWidget_target->SetRenderWindow(renderWindow_tgt);

	src_viewer.reset(new pcl::visualization::PCLVisualizer ("3D Viewer Source"));
	src_viewer->setBackgroundColor (0, 0, 0);
	src_viewer->addCoordinateSystem (1.0);
	src_viewer->initCameraParameters ();
	src_viewer->registerPointPickingCallback(&AlignmentDialog::src_pp_callback,*this, (void*)&tgt_viewer);
	vtkSmartPointer<vtkRenderWindow> renderWindow_src = src_viewer->getRenderWindow ();
	this->qvtkWidget_source->SetRenderWindow(renderWindow_src);

}
AlignmentDialog::~AlignmentDialog()
{
	tgt_viewer->removeAllPointClouds();
	tgt_viewer->removeAllShapes();
	tgt_viewer->close();

	src_viewer->removeAllPointClouds();
	src_viewer->removeAllShapes();
	src_viewer->close();

}

void AlignmentDialog::tgt_pp_callback (const pcl::visualization::PointPickingEvent &event,void*viewer_void)
{
	float x,y,z;
	size_t n = (size_t)event.getPointIndex();
	pcl::PointXYZ p = alignment->tgt_cloud->at(n);
	event.getPoint(p.x,p.y,p.z);
	
	stringstream ss;
	ss << alignment->tgt_points.size();
	string tgt_string = "tgt_point"+ss.str();
	tgt_viewer->addSphere(p,0.1,tgt_string);

	emit tgtPointPicked(n,p.x,p.y,p.z);

	populateListView(0);

}

void AlignmentDialog::src_pp_callback (const pcl::visualization::PointPickingEvent &event,void*viewer_void)
{
    size_t n = (size_t)event.getPointIndex();
	pcl::PointXYZ point = 	alignment->src_cloud->at(n);
	event.getPoint(point.x,point.y,point.z);

	//add sphere vo viewer
	stringstream ss;
	ss << alignment->src_points.size();
	string src_string = "src_point"+ss.str();
	src_viewer->addSphere(point,0.1,src_string);

	emit srcPointPicked(n,point.x,point.y,point.z);

	populateListView(0);

}

void AlignmentDialog::loadTargetCloud()
{
	alignment->loadTargetCloud(doubleSpinBox_targetLeafSizeX->value(),doubleSpinBox_targetLeafSizeY->value(),doubleSpinBox_targetLeafSizeZ->value());

}

void AlignmentDialog::nPointAlign()
{
	alignment->nPointTransformation();
}


void AlignmentDialog::loadSourceCloud()
{
	alignment->loadSourceCloud(doubleSpinBox_sourceLeafSizeX->value(),doubleSpinBox_sourceLeafSizeY->value(),doubleSpinBox_sourceLeafSizeZ->value());
}


void AlignmentDialog::removeTgtPoint()
{

	int i = listWidget_tgtPoints->currentIndex().row();
	alignment->tgt_points.erase( alignment->tgt_points.begin() + i );

	populateListView(0);

	stringstream ss;
	ss << alignment->tgt_points.size();
	string tgt_string = "tgt_point"+ss.str();
	tgt_viewer->removeShape(tgt_string);

}

void AlignmentDialog::removeSrcPoint()
{
	int i = listWidget_srcPoints->currentIndex().row();

	alignment->src_points.erase( alignment->src_points.begin() + i );
	populateListView(0);

	stringstream ss;
	ss << alignment->src_points.size();
	string src_string = "src_point"+ss.str();
	src_viewer->removeShape(src_string);


}



void AlignmentDialog::populateListView(int mode)
{
	listWidget_tgtPoints->clear();

	for(int i=0;i<alignment->tgt_points.size();i++)
	{
		QString pt = QString::number(i+1) + ": X: " + QString::number(alignment->tgt_points.at(i).x()) + ", Y: " + QString::number(alignment->tgt_points.at(i).y()) + ", Z: " + QString::number(alignment->tgt_points.at(i).z());
		
		listWidget_tgtPoints->addItem(new QListWidgetItem(pt));
	}

	listWidget_srcPoints->clear();
	for(int i=0;i<alignment->src_points.size();i++)
	{
		QString pt = QString::number(i+1) + ": X: " + QString::number(alignment->src_points.at(i).x()) + ", Y: " + QString::number(alignment->src_points.at(i).y()) + ", Z: " + QString::number(alignment->src_points.at(i).z());
		listWidget_srcPoints->addItem(new QListWidgetItem(pt));
	
	}

	if(alignment->src_points.size() == 0)
		pushButton_srcPointRemove->setEnabled(false);
	else
		pushButton_srcPointRemove->setEnabled(true);

	if(alignment->tgt_points.size() == 0)
		pushButton_tgtPointRemove->setEnabled(false);
	else
		pushButton_tgtPointRemove->setEnabled(true);

}